import lejos.nxt.*;
import lejos.robotics.navigation.*;
import lejos.robotics.localization.*;
/**
 * @(#)winbot9001.java
 *
 *
 * @author Sam Dietrich, Rick Gordon
 * @version 1.00 2012/1/13
 */


public class DumpBot12 {
	//Important movement stuff
	DifferentialPilot pilot;
	OdometryPoseProvider op;
	NavPathController control;
	//important claw position variable
	int clawAngle;
	int claw0;
    int claw1;
    int claw2;
    //important waypoint and action variables
	double[] x=    {38,38,  49,62.3,  88,  85,107,95};
    double[] y=    { 5,50,  63,56.8,  15,  24, 72,62};
    int[] flags=   { 0, 0,   2,   2,   1,   4,  1, 4}; //0=open 1=close 2=raise 4=let go and back up 8=fake


    public DumpBot12() {
    	clawAngle=Motor.A.getPosition();//open position
    	claw0=clawAngle;
    	claw1=clawAngle-50;
    	claw2=clawAngle-270;

    	pilot=new DifferentialPilot(5.7f,17.6f,Motor.B,Motor.C,true);
    	pilot.setAcceleration(55);//wheels
    	pilot.setTravelSpeed(22);//wheels
    	pilot.setRotateSpeed(25);//wheels
    	Motor.A.setSpeed(130);//claw
    	op=new OdometryPoseProvider(pilot);
    	pilot.addMoveListener(op);//update position when not using the NPC
    	control=new NavPathController(pilot,op);
    }

    public void go(){
    	for(int i=0;i<x.length;i++){
    		control.goTo(x[i],y[i]);
    		claw(flags[i]);
    	}
    	pilot.setAcceleration(100);
    	pilot.setTravelSpeed(50);
    	pilot.travel(-70);
    }

    public void claw(int pos){
    	switch(pos){
    		case 0:{
    			Motor.A.rotateTo(claw0);
    			clawAngle=claw0;
    		}break;
    		case 1:{
    			Motor.A.rotateTo(claw1);
    			clawAngle=claw1;
    		}break;
    		case 2:{
    			Motor.A.rotateTo(claw2);
    			clawAngle=claw2;
    			Motor.A.rotateTo(claw0);
				clawAngle=claw0;
				pilot.travel(4);
				Motor.A.rotateTo(claw2);
    			clawAngle=claw2;
    			Motor.A.rotateTo(claw0);
				clawAngle=claw0;
    		}break;
    		case 4:{
    			Motor.A.rotateTo(claw0);
				clawAngle=claw0;
    			pilot.travel(-15);
    		}break;
    		case 8:{
    			Motor.A.rotateTo(claw0);
				clawAngle=claw0;
    		}break;
    	}

    }

    public static void main(String[] args){

    	DumpBot12 RAWR=new DumpBot12();
    	RAWR.go();
    }


}